#include "Matrix4.h"

Matrix4::Matrix4(void)
{
    for(int i=0 ; i<get_rows() ; i++)
        for(int j=0 ; j<get_columns() ; j++)
        {
            if (i == j)
                (*this)(i,j) = 1;
            else
                (*this)(i,j) = 0;
        }
    //cout<<"* Matriz "<<get_rows()<<"x"<<get_columns()<<" criada!"<<endl;
}

void Matrix4::imprimir(void)
{
    for(int i=0 ; i<(get_rows()) ; i++)
    {
        for(int j=0 ; j<(get_columns()) ; j++)
            cout <<(*this)(i,j)<<"\t";
        cout<<endl;
    }
}

Matrix4 Matrix4::operator+(const Matrix4 & other) const
{
    Matrix4 aux;
    for(int i=0 ; i<get_rows() ; i++)
        for(int j=0 ; j<get_columns() ; j++)
            aux(i,j) = (*this)(i,j) + other(i,j);
    return(aux);
}

Matrix4 Matrix4::operator-(const Matrix4 & other) const
{
    Matrix4 aux;
    for(int i=0 ; i<get_rows() ; i++)
        for(int j=0 ; j<get_columns() ; j++)
            aux(i,j) = (*this)(i,j) - other(i,j);
    return(aux);
}

Matrix4 Matrix4::operator*(const Matrix4 & other) const
{
    Matrix4 aux;
    // Checar se é possivel fazer o produto:
    if (get_columns() != other.get_rows())
        cout << "O produto entre as matrizes nao e possivel!" <<endl;
    else
    {
        // Cálculo do produto das matrizes:
        for(int i=0 ; i<get_rows() ; i++)
            for(int j=0 ; j<other.get_columns() ; j++)
            {
                aux(i,j) = 0;
                for(int k=0 ; k<get_columns() ; k++)
                    aux(i,j) += (*this)(i,k) * other(k,j);
            }
    }
    return(aux);
}

Vector3 Matrix4::operator*(const Vector3 & vector) const
{
    Vector3 aux;
    for(int i=0 ; i<get_rows() ; i++)
    {
        aux(i) = 0;
        for(int k=0 ; k<get_columns() ; k++)
            aux(i) += (*this)(i,k) * vector(k);
    }
    return(aux);
}

Point3 Matrix4::operator*(const Point3 & point) const
{
    Point3 aux;
    for(int i=0 ; i<get_rows() ; i++)
    {
        aux(i) = 0;
        for(int k=0 ; k<get_columns() ; k++)
            aux(i) += (*this)(i,k) * point(k);
    }
    return(aux);
}


/// Funções extras ///
Matrix4 translationMatrix(Vector3 displacement)
{
    Matrix4 aux;
    for(int i=0 ; i<aux.get_rows()-1 ; i++)
        aux(i,aux.get_columns()-1) = displacement(i);
    return(aux);
}

Matrix4 scaleMatrix(float x, float y, float z)
{
    Matrix4 aux;
    aux(0,0) = x;
    aux(1,1) = y;
    aux(2,2) = z;
    return(aux);
}

Matrix4 rotationMatrix(float angle, Vector3 axis)
{
    Matrix4 aux;
    // Normalizando o vetor "axis":
    Vector3 u = axis.normalize();
    // Transformando o ângulo "angle" para radianos:
    angle = rad(angle);
    angle /= 2;

    /// Usaremos quatérnio. ///

    // Membros do quatérnio:
    float x = sin(angle) * u(0);
    float y = sin(angle) * u(1);
    float z = sin(angle) * u(2);
    float w = cos(angle);

    // Preenchendo a matriz do quatérnio de rotação:
    aux(0,0) = 1 - 2*y*y - z*z;
    aux(0,1) = 2*x*y - 2*w*z;
    aux(0,2) = 2*x*z + 2*w*y;
    //aux(0,3);
    aux(1,0) = 2*x*y + 2*w*z;
    aux(1,1) = 1 - 2*x*x - z*z;
    aux(1,2) = 2*y*z - 2*w*x;
    //aux(1,3);
    aux(2,0) = 2*x*z - 2*w*y;
    aux(2,1) = 2*y*z + 2*w*x;
    aux(2,2) = 1 - 2*x*x - y*y;
    //aux(2,3);
    /*aux(3,0);
    aux(3,1);
    aux(3,2);
    aux(3,3);*/

    /*** DEBUG *********************
    cout<<"angle = "<<angle<<endl;
    cout<<"u     = ";u.imprimir();
    cout<<"x = "<<x<<endl;
    cout<<"y = "<<y<<endl;
    cout<<"z = "<<z<<endl;
    cout<<"w = "<<w<<endl;
    ********************************/

    return(aux);
}

Matrix4 shearMatrix(float angle)
{
    Matrix4 aux;
    aux(0,1) = 1/tan(rad(angle));
    return(aux);
}

float rad(float angulo)
{
    return(angulo*(M_PI/180));
}
